/**********************************************************************
 Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/
#ifndef UNITREEROBOT_H
#define UNITREEROBOT_H

#include "common/unitreeLeg.h"
#include "message/LowlevelState.h"

class QuadrupedRobot{
public:
    QuadrupedRobot(){};
    ~QuadrupedRobot(){}


    /// @brief 位置正运动学, 计算0号腿(FR)在{body}系中的足端位置
    /// @param state 
    /// @return 足端位置
    Vec3 getX(LowlevelState &state);

    /// @brief 以0号足足端为原点,以平行于{body}的xyz轴建立{s}, 计算在{s}下4足足端坐标
    /// @param state 
    /// @return {s}下4足足端坐标
    Vec34 getVecXP(LowlevelState &state);


    /// @brief 位置逆运动学, 根据4条腿在{frame}下的坐标计算出12个关节的角度
    /// @param feetPosition 4条腿在{frame}下的坐标
    /// @param frame feetP所在的坐标系,可选FrameType::BODY||HIP
    /// @return 12个关节的角度, 顺序参考教材P25
    Vec12 getQ(const Vec34 &feetPosition, FrameType frame);

    /// @brief 速度逆运动学, 根据4条腿在{frame}下的坐标、速度计算出12个关节的角速度
    /// @param feetPosition 4条腿在{frame}下的坐标
    /// @param feetVelocity 4条腿在{frame}下的速度
    /// @param frame 坐标、速度所在的坐标系,可选FrameType::BODY||HIP
    /// @return 12个关节的角度
    Vec12 getQd(const Vec34 &feetPosition, const Vec34 &feetVelocity, FrameType frame);

    /// @brief 逆向静力学, 根据12个关节角和4个足端力计算12个关节的力矩
    /// @param q 12个关节角
    /// @param feetForce 4个足端力
    /// @return 12个关节的力矩
    Vec12 getTau(const Vec12 &q, const Vec34 feetForce);


    /// @brief 位置正运动学, 计算一条腿(在{frame}中)当前足端位置
    /// @param state lowstate
    /// @param id 腿编号
    /// @param frame 足端位置所在的坐标系,可选FrameType::BODY||HIP
    /// @return 足端位置
    Vec3 getFootPosition(LowlevelState &state, int id, FrameType frame);

    /// @brief 速度正运动学, 计算一条腿当前足端速度 (在{body}/{hip}中相同, 因为两个系没有相对速度/角速度)
    /// @param state lowstate
    /// @param id 腿编号
    /// @return 足端速度
    Vec3 getFootVelocity(LowlevelState &state, int id);


    /// @brief 位置正运动学, 计算4条腿(在{frame}中)当前足端位置
    /// @param state lowstate
    /// @param frame 足端位置所在的坐标系,可选FrameType::BODY||HIP||GLOBAL
    /// @return 足端位置
    Vec34 getFeet2BPositions(LowlevelState &state, FrameType frame);


    /// @brief 速度正运动学, 计算4条腿(在{frame}中)当前足端速度
    /// @param state lowstate
    /// @param frame 足端速度所在的坐标系,可选FrameType::BODY||HIP||GLOBAL
    /// @return 足端速度
    Vec34 getFeet2BVelocities(LowlevelState &state, FrameType frame);


    /// @brief 获取一条腿当前的jacob矩阵
    /// @param state lowstate
    /// @param legID 腿编号
    /// @return 3x3 jacob
    Mat3 getJaco(LowlevelState &state, int legID);

    /*get property*/

    /// @brief 获取x方向速度区间
    /// @return 
    Vec2 getRobVelLimitX(){return _robVelLimitX;}

    /// @brief 获取y方向速度区间
    /// @return
    Vec2 getRobVelLimitY(){return _robVelLimitY;}

    /// @brief 获取yaw轴旋转角速度区间
    /// @return
    Vec2 getRobVelLimitYaw(){return _robVelLimitYaw;}

    /// @brief 获取足端中性落脚点在{b}下位置
    /// @return
    Vec34 getFeetPosIdeal(){return _feetPosNormalStand;}

    /// @brief 获取机器人简化模型质量
    /// @return
    double getRobMass(){return _mass;}

    /// @brief 获取 ??质心在{b}下坐标??
    /// @return
    Vec3 getPcb(){return _pcb;}

    /// @brief 简化模型在{b}下的转动惯量
    /// @return
    Mat3 getRobInertial(){return _Ib;}

protected:
    QuadrupedLeg* _Legs[4]; //数组, 指向4条腿的实例
    Vec2 _robVelLimitX;     //(在{b}下) x方向速度区间(m/s?)
    Vec2 _robVelLimitY;     //(在{b}下) y方向速度区间(m/s?)
    Vec2 _robVelLimitYaw;   //(在{b}下) yaw轴旋转角速度区间(rad/s?)
    Vec34 _feetPosNormalStand;  //足端中性落脚点在{b}下位置, 步态规划用
    double _mass;   //机器人简化模型质量
    Vec3 _pcb;      //??质心在{b}下坐标??
    Mat3 _Ib;       //简化模型在{b}下的转动惯量
};

class A1Robot : public QuadrupedRobot{
public:
    A1Robot();
    ~A1Robot(){}
};

class Go1Robot : public QuadrupedRobot{
public:
    Go1Robot();
    ~Go1Robot(){};
};

#endif  // UNITREEROBOT_H